package org.visionlibrary.image.geomtric.lines.merge;

import static java.lang.Math.cos;
import static java.lang.Math.sin;

import java.util.ArrayDeque;
import java.util.ArrayList;
import java.util.Deque;
import java.util.List;

import org.visionlibrary.image.geomtric.lines.Lines;
import org.visionlibrary.image.geomtric.lines.merge.model.LineMerge;
import org.visionlibrary.image.geomtric.model.Point2d;
import org.visionlibrary.image.geomtric.model.Vector2d;


/**
 * Klasa realizujaca algorytm scalania metoda opracowana przez panow Tavares i 
 * Padilha. Jej dzialanie jest opisane w pracy pisemnej.
 */
public class TavaresPadilhaSegmentMerge implements LineMerge {
	//precyzja dla liczb typu double
	public static final double DOUBLE_PREC = 1e-5;

	//maksymalna odleglosc od siebie dwoch odcinkow ktore sa scalane
	//odleglosc mierzona jest na osi X
	private double maximumXgAxisDistance = 0;

	//maksymalna odleglosc na osi Y dla odcinkow ktore nachodza na siebie
	//calkowicie (jeden z odcinkow jest krotszy jeden dluzszy)
	
	private double maximumYgAxisDistanceForOverlapping = 0;
	
	//maksymalna odleglosc na osi Y dla odcinkow ktore nie nachodza na siebie
	private double maximumYgAxisDistanceForNonOverlapping = 0;
	
	//maksymalna odleglosc na osi Y dla odcinkow ktore nachodza na siebie czesciowo
	private double maximumYgAxisDistanceForPartiallyOverlapping = 0;

	//maksymalna roznica katow nachylenia dwoch odcinkow
	private double maximumAngleDifference = 0;
	
	public TavaresPadilhaSegmentMerge(
			double maximumAngleDifference,
			double maximumXgAxisDistance,
			double maximumYgAxisDistanceForOverlapping,
			double maximumYgAxisDistanceForNonOverlapping,
			double maximumYgAxisDistanceForPartiallyOverlapping
			) {
		this.maximumAngleDifference = maximumAngleDifference;
		this.maximumXgAxisDistance = maximumXgAxisDistance;
		this.maximumYgAxisDistanceForOverlapping = maximumYgAxisDistanceForOverlapping;
		this.maximumYgAxisDistanceForNonOverlapping = maximumYgAxisDistanceForNonOverlapping;
		this.maximumYgAxisDistanceForPartiallyOverlapping = maximumYgAxisDistanceForPartiallyOverlapping;
	}

	@Override
	public List<Vector2d> merge(List<Vector2d> lines) {
		if (null == lines)
			throw new NullPointerException("Line list is null.");

		if (lines.isEmpty())
			throw new IllegalArgumentException("Line list is empty.");

		if (1 == lines.size())
			return lines;

		List<Vector2d> merged = new ArrayList<Vector2d>();
		List<Vector2d> linelist = new ArrayList<Vector2d>();

		linelist.addAll(lines);
		Lines.sortDescendingByAngle(linelist);

		while (!linelist.isEmpty()) {
			Deque<Vector2d> deque = new ArrayDeque<Vector2d>(linelist);
			Vector2d line1 = deque.pollFirst();
			linelist.remove(line1);
			Vector2d mergedline = null;
			int notMerged = 0;
			while (!deque.isEmpty()) {
				Vector2d line2 = deque.pollFirst();
				if (Math.abs(line1.angle() - line2.angle()) > maximumAngleDifference){
					continue;
				}
					
				mergedline = merge2lines(line1, line2);
				if (null != mergedline) {
					linelist.remove(line2);
					line1 = mergedline;
					notMerged = 0;
				}else{
					deque.addLast(line2);
					notMerged++;
				}
				
				if(notMerged >= deque.size()){
					break;
				}
			}
			if (null == mergedline) {
				merged.add(line1);
			} else {
				merged.add(mergedline);
			}
		}

		return merged;
	}
	
	public Vector2d merge2lines(Vector2d vector1, Vector2d vector2) {
		double fii = vector1.angle();
		double li = vector1.length();
		double ax = vector1.getFirst().getX();
		double ay = vector1.getFirst().getY();
		double bx = vector1.getLast().getX();
		double by = vector1.getLast().getY();

		double fij = vector2.angle();
		double lj = vector2.length();
		double cx = vector2.getFirst().getX();
		double cy = vector2.getFirst().getY();
		double dx = vector2.getLast().getX();
		double dy = vector2.getLast().getY();

		double xcentroid = (li * (ax + bx) + lj * (cx + dx)) / (2 * (li + lj));
		double ycentroid = (li * (ay + by) + lj * (cy + dy)) / (2 * (li + lj));
		double fir = 0d; // wartosc kata o ktora zostanie dokonana translacja
							// ukladu

		// roznica katow pomiedzy odcinkami nam nie pasuje
		if (Math.abs(fii - fij) > maximumAngleDifference)
			return null;

		// dopuszczalna roznica katow nie musi byc mniejsza niz pi/2
		// wiec uwzglednione zostaly dwa przypadki dla roznicy mniejszej niz
		// pi/2
		// oraz roznicy wiekszej niz pi/2
		if (Math.abs(fii - fij) <= Math.PI / 2)
			fir = (li * fii + lj * fij) / (li + lj);
		else
			fir = ((li * fii) + lj * (fij - Math.PI * (fij / Math.abs(fij))))
					/ (li + lj);

		double tax = translateX(ax, ay, fir, xcentroid, ycentroid);
		double tay = translateY(ax, ay, fir, xcentroid, ycentroid);

		double tbx = translateX(bx, by, fir, xcentroid, ycentroid);
		double tby = translateY(bx, by, fir, xcentroid, ycentroid);

		double tcx = translateX(cx, cy, fir, xcentroid, ycentroid);
		double tcy = translateY(cx, cy, fir, xcentroid, ycentroid);

		double tdx = translateX(dx, dy, fir, xcentroid, ycentroid);
		double tdy = translateY(dx, dy, fir, xcentroid, ycentroid);

		double minOnTranslatedX = Double.MAX_VALUE;
		minOnTranslatedX = Math.min(minOnTranslatedX, tax);
		minOnTranslatedX = Math.min(minOnTranslatedX, tbx);
		minOnTranslatedX = Math.min(minOnTranslatedX, tcx);
		minOnTranslatedX = Math.min(minOnTranslatedX, tdx);

		double maxOnTranslatedX = Double.MIN_VALUE;
		maxOnTranslatedX = Math.max(maxOnTranslatedX, tax);
		maxOnTranslatedX = Math.max(maxOnTranslatedX, tbx);
		maxOnTranslatedX = Math.max(maxOnTranslatedX, tcx);
		maxOnTranslatedX = Math.max(maxOnTranslatedX, tdx);

		double minx = retranslateX(xcentroid, minOnTranslatedX, 0, fir);
		double miny = retranslateY(ycentroid, minOnTranslatedX, 0, fir);

		double maxx = retranslateX(xcentroid, maxOnTranslatedX, 0, fir);
		double maxy = retranslateY(ycentroid, maxOnTranslatedX, 0, fir);

		Vector2d merged = new Vector2d(minx, miny, maxx, maxy);

		double tablen = Point2d.distance(new Point2d(tax, tay), new Point2d(
				tbx, tby));
		double tcdlen = Point2d.distance(new Point2d(tcx, tcy), new Point2d(
				tdx, tdy));

		double minOnTranslatedY = Double.MAX_VALUE;
		minOnTranslatedY = Math.min(minOnTranslatedY, tay);
		minOnTranslatedY = Math.min(minOnTranslatedY, tby);
		minOnTranslatedY = Math.min(minOnTranslatedY, tcy);
		minOnTranslatedY = Math.min(minOnTranslatedY, tdy);

		double maxOnTranslatedY = Double.MIN_VALUE;
		maxOnTranslatedY = Math.max(maxOnTranslatedY, tay);
		maxOnTranslatedY = Math.max(maxOnTranslatedY, tby);
		maxOnTranslatedY = Math.max(maxOnTranslatedY, tcy);
		maxOnTranslatedY = Math.max(maxOnTranslatedY, tdy);

		double tyglen = Point2d.distance(new Point2d(0, minOnTranslatedY),
				new Point2d(0, maxOnTranslatedY));

		// zlaczane odcinki nakladaja sie wiec trzeba sprawdzic odleglosc na osi
		// Yg ze wzgledu na operacje na liczbach zmienno pozycyjnych sprawdzam
		// roznice wartosci czy mniejsza jest od wybranej dokladnosci
		
		if (Math.abs(tablen - merged.length()) < DOUBLE_PREC
				|| Math.abs(tcdlen - merged.length()) < DOUBLE_PREC) {
			if (tyglen <= maximumYgAxisDistanceForOverlapping)
				return merged;

			return null;
		}

		// zlaczane odcinki wogole sie nie nakladaja sprawdzamy jaka jest
		// odleglosc miedzy
		// rzutami na os Xg odcinkow w nowym ukladzie a dlugoscia nowego odcinka
		// jezeli spelni warunek sprawdzamy to samo na osi Yg
		if (merged.length() >= (tablen + tcdlen)) {
			if ((merged.length() - (tablen + tcdlen)) <= maximumXgAxisDistance
					&& tyglen <= maximumYgAxisDistanceForNonOverlapping)
				return merged;
		} else {
			// zlaczane odcinki tylko czesciowo sie nakladaja sprawdzamy wiec
			// tylko odleglosc
			// na osi Yg
			if (tyglen <= maximumYgAxisDistanceForPartiallyOverlapping)
				return merged;
		}

		return null;
	}

	private double translateX(double oldx, double oldy, double angle,
			double xcentroid, double ycentroid) {
		return (oldy - ycentroid) * sin(angle) + (oldx - xcentroid)
				* cos(angle);
	}
	
	/*
	 * oldx = 0, oldy = 1, angle = PI/2, xc = 0, yc = 0
	 * (1 - 0) * 1 + (0 - 0) * 0 = 1
	 * 
	 * 
	 * (1 - 0) * 0 - (0 - 0) * 1 = 0
	 */

	private double translateY(double oldx, double oldy, double angle,
			double xcentroid, double ycentroid) {
		return (oldy - ycentroid) * cos(angle) - (oldx - xcentroid)
				* sin(angle);
	}

	private double retranslateX(double xcentroid, double translatedx,
			double translatedy, double angle) {
		double sin2 = sin(angle) * sin(angle);
		double cos2 = cos(angle) * cos(angle);
		return (xcentroid + ((translatedx * cos(angle)) / (sin2 + cos2)) - ((translatedy * sin(angle)) / (sin2 + cos2)));
	}

	private double retranslateY(double ycentroid, double translatedx,
			double translatedy, double angle) {
		double sin2 = sin(angle) * sin(angle);
		double cos2 = cos(angle) * cos(angle);
		return (ycentroid + ((translatedx * sin(angle)) / (sin2 + cos2)) + ((translatedy * cos(angle)) / (sin2 + cos2)));
	}
}
